/**
 * Project Babau
 * Hand Joystick module
 *
 * @author Giuliano Vilela <giulianoxt@gmail.com>
 */

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include "ui.hpp"
#include "cvutil.hpp"
#include "optflow.hpp"
#include "handstick.hpp"
#include "parameters.hpp"

#include <cmath>
#include <string>
#include <vector>
#include <iostream>
#include <algorithm>
using namespace std;


enum main_state_t {
  STAND_BY,
  CALIBRATE,
  TRACK_POINTS
};


int main(int argc, char* argv[])
{
  cout << "* Babau Handstick *" << endl;

  if (argc != 2) {
    cout << "usage: babau window_name" << endl;
    return 1;
  }

  FrameSource source;
  WindowController* controller = WindowController::create(argv[1]);

  if (!source.setup()) {
    cerr << "Could not initialize webcam...";
    return 1;
  }

  cv::namedWindow(MAIN_WINDOW, CV_WINDOW_AUTOSIZE | CV_GUI_NORMAL);
  //cv::namedWindow(DEBUG_WINDOW, CV_WINDOW_AUTOSIZE | CV_GUI_NORMAL);

  cv::displayOverlay(MAIN_WINDOW, MSG_INITIAL, MSG_TIMEOUT);

  
  cv::Mat image;
  cv::Mat frame;
  cv::Mat previous_frame;
  cv::Size frame_size;
  cv::timer calibration_timer;

  vector<cv::Point2f> cur_points;
  vector<cv::Point2f> next_points;

  source.update();
  frame_size = source.get_clean_frame().size();

  
  main_state_t state = STAND_BY;

  while (true) {
    source.update();
    image = source.get_frame();
    frame = source.get_clean_frame();
    previous_frame = source.get_previous_clean_frame();

    if (frame.empty()) {
      cerr << "Some problem ocurred with the webcam connection" << endl;
      return 1;
    }

    int key = cv::waitKey(1000.0 / FPS);

    if (key == ESC_CODE) {
      break;
    }
    else if (key == 'c') {
      cur_points.clear();
      cv::init_timer(calibration_timer);

      cv::displayOverlay(MAIN_WINDOW, MSG_CALIBRATE, CALIBRATION_TIMEOUT*1000);
      state = CALIBRATE;
    }

    if (state == STAND_BY)
    {
      // wait for calibration
    }
    else if (state == CALIBRATE)
    {
      draw_calibration_rects(image, calibration_timer);

      if (cv::seconds_spent(calibration_timer) >= CALIBRATION_TIMEOUT) {
        
        cv::Rect left_roi, right_roi;
        get_calibration_rects(frame, left_roi, right_roi);

        cv::Point2f left_hint  = cv::rect_center(left_roi);
        cv::Point2f right_hint = cv::rect_center(right_roi);

        cur_points = find_hands_corners(frame, left_hint, right_hint);

        cout << "using " << cur_points << endl;

        controller->tracking_started();
        state = TRACK_POINTS;
      }
    }
    else if (state == TRACK_POINTS)
    {
      bool found_pts = track_hands(
        previous_frame, frame,
        cur_points, next_points
      );

      if (found_pts) {
        HandState hand_st = HandState::create(next_points[0], next_points[1]);

        controller->update(hand_st);

        cv::Point2i pt_a = map_point(next_points[0], frame, image);
        cv::Point2i pt_b = map_point(next_points[1], frame, image);

        draw_ui(image, pt_a, pt_b, hand_st);

        cur_points = next_points;
        next_points.clear();
      }
      else {
        controller->tracking_lost();

        cv::displayOverlay(MAIN_WINDOW, MSG_RECALIBRATE, CALIBRATION_TIMEOUT*1000);
        cur_points.clear();
        cv::init_timer(calibration_timer);
        state = CALIBRATE;
      }
    }

    cv::imshow(MAIN_WINDOW, image);
  }

  return 0;
}
